
// 1. 核心控制逻辑说明：

// 中心点接收：订阅由<center_detection>功能包发布的中心点坐标，并对其进行json解析
// 点云滤波：采用移动平均滤波函数对中心点累积取平均，用来计算距离
// 偏移计算：根据当前中心点坐标计算其与坐标系原点之间的距离
// 控制阶段：根据偏移量计算相应的速度指令
// 在速度控制阶段，加入匀加速度函数进行平滑控制

// 2. 速度调整策略：

// X偏移 → 控制前进/后退速度
// Y偏移 → 控制左右平移速度
// 角度偏移（无需角度偏移）


#include "back_local_navigation/local_navigator.hpp"
#include <jsoncpp/json/json.h>
#include <rclcpp/rclcpp.hpp>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <deque>

namespace back_local_navigation {

    LocalNavigator::LocalNavigator(const rclcpp::NodeOptions & options)
        : Node("back_local_navigator", options),
        current_center_x_(0.0),
        current_center_y_(0.0),
        current_center_z_(0.0),
        target_detected_(false),
        x_buffer_(),
        y_buffer_(),
        z_buffer_()
    {
        // 声明并获取参数
        center_json_topic_ = this->declare_parameter("center_json_topic", "/neutron_center_json");
        cmd_vel_topic_ = this->declare_parameter("cmd_vel_topic", "/cmd_vel");
        reached_topic_ = this->declare_parameter("reached_topic", "/back_navigation_reached");

        max_linear_velocity_ = this->declare_parameter("max_linear_velocity", 0.1); 

        y_position_tolerance_ = this->declare_parameter("y_position_tolerance", 0.01); 

        x_position_tolerance_ = this->declare_parameter("x_position_tolerance", 0.02); 

        FILTER_WINDOW_SIZE_ = this->declare_parameter("filter_window_size", 3);

        controlling_frequency_ = this->declare_parameter("controlling_frequency", 200);
        
        max_acceleration_ = this->declare_parameter("max_acceleration", 0.1);
        max_angular_acceleration_ = this->declare_parameter("navigation.max_angular_acceleration", 0.1);



        // 创建发布者和订阅者
        center_json_sub_ = this->create_subscription<std_msgs::msg::String>(center_json_topic_, 10,
            std::bind(&LocalNavigator::centerJsonCallback, this, std::placeholders::_1));

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>(cmd_vel_topic_, 10);

        reached_pub_ = this->create_publisher<std_msgs::msg::Bool>(reached_topic_, rclcpp::QoS(1).transient_local());
        
        // 创建控制循环定时器 
        control_timer_ = this->create_wall_timer(std::chrono::milliseconds(controlling_frequency_),
            std::bind(&LocalNavigator::controlLoop, this));

        RCLCPP_INFO(this->get_logger(), "Fine Adjustment Controller initialized.");
    }


    // 解析、传递订阅的放射源中心点坐标
    void LocalNavigator::centerJsonCallback(const std_msgs::msg::String::SharedPtr msg)
    {
        
        if(center_coords.parse_json_string(msg->data))
        {
            current_center_x_ = center_coords.get_coord_x();
            current_center_y_ = center_coords.get_coord_y();
            current_center_z_ = center_coords.get_coord_z();

            // 将新值添加到缓冲区
            x_buffer_.push_back(current_center_x_);
            y_buffer_.push_back(current_center_y_);
            z_buffer_.push_back(current_center_z_);

            // 如果缓冲区大小超过窗口大小，则移除最旧的元素
            if (x_buffer_.size() > FILTER_WINDOW_SIZE_)
            {
                x_buffer_.pop_front();
                y_buffer_.pop_front();
                z_buffer_.pop_front();
            }

            target_detected_ = true;
        }
        else
        {
            target_detected_ = false;
        }

    }


    // 控制loop
    void LocalNavigator::controlLoop()
    {

        // 放射源点云中心坐标均值定义
        double avg_x = 0.0, avg_y = 0.0, avg_z = 0.0;

        if (!target_detected_) 
        {
            // 没有检测到目标，停止
            /* linear_x = 0.0;
            linear_y = 0.0;
            angular_z = 0.0; */
            applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
            
            RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No target detected");
        } else 
        {
            // 只有当缓冲区中有数据才进行计算
            if(!x_buffer_.empty()){
                // 计算缓冲区中 X， Y， Z 坐标的平均值
                for(double val : x_buffer_) {avg_x += val;}
                for(double val : y_buffer_) {avg_y += val;}
                for(double val : z_buffer_) {avg_z += val;}
                avg_x /= x_buffer_.size();
                avg_y /= y_buffer_.size();
                avg_z /= z_buffer_.size();

                calculateVelocityFromDistance(linear_x, linear_y, angular_z, avg_x, avg_y);
            }

        }


        vel_cmd.set_from_twist(linear_x, linear_y, angular_z);
        std::string json_string = vel_cmd.get_json_string();

        if(!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            cmd_vel_pub_->publish(msg);
        }


        // 机器人到达指定地点后，发布一个值为true的话题，否则发布false
        std_msgs::msg::Bool reached_msg;
        if (target_detected_ && !x_buffer_.empty()) {
            double y_distance = std::abs(avg_y);
            double x_distance = std::abs(avg_x);
            // 防止抖动
            static int stable_count = 0;
            if(y_distance <= y_position_tolerance_ && x_distance <= x_position_tolerance_)
            {
                stable_count ++;
            } else {
                stable_count = 0;
            }
            reached_msg.data = (stable_count >= 3); // 连续true三次才算到达
        } else {
            reached_msg.data = false;
        }
        reached_pub_->publish(reached_msg);

    }



    // 根据放射源中心点与原点距离输出x、y轴速度分量
    // 添加了avg_x、avg_y作为输入参数
    // 导航逻辑：
    // 先计算点云中心y坐标和机器人中心的横向距离，输出横向速度，y轴距离差限制在阈值之内；
    // 再计算y轴距离差，输出前向速度，将距离差限制在阈值之内；
    // *不调整角速度；动态调整加减速度值。
    void LocalNavigator::calculateVelocityFromDistance(double& linear_x, double& linear_y, double& angular_z, double avg_x, double avg_y)
    {

        double y_distance = std::abs(avg_y);
        double x_distance = std::abs(avg_x);

        // 根据距离调整速度：距离越近，速度越小
        double y_speed_factor = std::min(1.0, y_distance / y_position_tolerance_); // 速度因子：0-1
        double x_speed_factor = std::min(1.0, x_distance / x_position_tolerance_);

        // 分阶段导航：先校准y轴距离，再调整x轴方向

        // 根据y轴距离差优先施加横向速度
        if(y_distance > y_position_tolerance_){
            target_linear_x = 0.0;
            target_linear_y = max_linear_velocity_ * y_speed_factor * (avg_y > 0 ? 1 : -1);
            target_angular_z = 0.0;
            applyVelocitySmoothing(linear_x, linear_y, angular_z, target_linear_x, target_linear_y, target_angular_z);
            return;
        }
        else {
            // y轴校准完成，更新校准完成标志位
            static bool has_y_calibrated_ = false;
            if(!has_y_calibrated_){
                RCLCPP_INFO(this->get_logger(),"y轴方向校准完成, 下面即将前行！");
                has_y_calibrated_ = true;
            }
            
            // 横向校准完成后，再前进
            if(x_distance > x_position_tolerance_){
                target_linear_x = max_linear_velocity_ * x_speed_factor * (avg_x > 0 ? 1 : -1);
                target_linear_y = 0.0;
                target_angular_z = 0.0;
                applyVelocitySmoothing(linear_x, linear_y, angular_z, target_linear_x, target_linear_y, target_angular_z);
                return;
            }
            else {
                // x、y轴都到达目标
                target_linear_x = 0.0; 
                target_linear_y = 0.0;
                target_angular_z = 0.0;
                applyVelocitySmoothing(linear_x, linear_y, angular_z, 0.0, 0.0, 0.0);
                RCLCPP_INFO(this->get_logger(),"机器人已到达指定位置！");
                has_y_calibrated_ = false;
            }
               
        }
    
    }


    // 速度平滑函数
    void LocalNavigator::applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                            double target_linear_x, double target_linear_y, double target_angular_z) {
        
        // 计算时间间隔:s
        double dt = controlling_frequency_ / 1000; 
        
        // 计算线性速度的最大变化量
        double max_linear_change = max_acceleration_ * dt;
        double max_angular_change = max_angular_acceleration_ * dt;
        
        // 计算实际速度变化（限制在最大变化范围内）
        double dx_change = std::clamp(target_linear_x - prev_linear_x_, 
                                    -max_linear_change, max_linear_change);
        double dy_change = std::clamp(target_linear_y - prev_linear_y_, 
                                    -max_linear_change, max_linear_change);
        double dtheta_change = std::clamp(target_angular_z - prev_angular_z_, 
                                        -max_angular_change, max_angular_change);
        
        // 更新目标速度
        linear_x = prev_linear_x_ + dx_change;
        linear_y = prev_linear_y_ + dy_change;
        angular_z = prev_angular_z_ + dtheta_change;
        
        // 更新上一次的速度值
        prev_linear_x_ = linear_x;
        prev_linear_y_ = linear_y;
        prev_angular_z_ = angular_z;
    }

} 

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<back_local_navigation::LocalNavigator>());
    rclcpp::shutdown();
    return 0;
}